#include "mcp23s17.h"
#include <string.h>

#define RD_BIT 1
#define WR_BIT 0
#define RD_CMD(dev_adr) (0x40 | (((dev_adr) << 1) & 0xE) | RD_BIT)
#define WR_CMD(dev_adr) (0x40 | (((dev_adr) << 1) & 0xE) | WR_BIT)

typedef union {
    uint16_t word;
    uint8_t byte[2];
    struct
    {
        uint16_t lobt : 8;
        uint16_t hibt : 8;
    };
} reg_data_t;

__weak int32_t mcp_spirw_buf(uint8_t *tbuf, uint8_t *rbuf, uint8_t len)
{
    return 0;
}
static void DelayNop(void)
{
    int i = 5;
    while (i--)
    {
        __nop();
    }
}

int32_t mcp23s17_init(mcp23s17_t *p, uint8_t id, uint16_t dir, uint16_t def)
{
    reg_data_t *pdir = (reg_data_t *)&dir;
    reg_data_t *pdef = (reg_data_t *)&def;
    /* set id*/
    memset(p, 0, sizeof(mcp23s17_t));
    /* Enables the MCP23S17 address pins. */
    p->iocona.bits.HAEN = 1;
    p->ioconb.bits.HAEN = 1;
    mcp23s17_write(p, BANK0_IOCONA, p->iocona.byte);
    DelayNop();
    mcp23s17_write(p, BANK0_IOCONB, p->ioconb.byte);
    DelayNop();
    /* set id*/
    p->devaddr = id;
    /* read iocon */
    mcp23s17_read(p, BANK0_IOCONA, (uint8_t *)&p->iocona.byte);
    DelayNop();
    mcp23s17_read(p, BANK0_IOCONB, (uint8_t *)&p->ioconb.byte);
    DelayNop();
    if (p->iocona.bits.HAEN != 1 || p->ioconb.bits.HAEN != 1)
        return -1;
    mcp23s17_write(p, BANK0_DEFVALA, pdef->lobt);
    DelayNop();
    mcp23s17_write(p, BANK0_DEFVALB, pdef->hibt);
    DelayNop();
    mcp23s17_read(p, BANK0_DEFVALA, (uint8_t *)&p->defvala.byte);
    DelayNop();
    mcp23s17_read(p, BANK0_DEFVALA, (uint8_t *)&p->defvalb.byte);
    DelayNop();
    if (pdef->lobt != p->defvala.byte || pdef->hibt != p->defvalb.byte)
        return -2;

    mcp23s17_write(p, BANK0_IODIRA, pdir->lobt);
    DelayNop();
    mcp23s17_write(p, BANK0_IODIRB, pdir->hibt);
    DelayNop();
    mcp23s17_read(p, BANK0_IODIRA, (uint8_t *)&p->iodira.byte);
    DelayNop();
    mcp23s17_read(p, BANK0_IODIRB, (uint8_t *)&p->iodirb.byte);
    DelayNop();
    if (pdir->lobt != p->iodira.byte || pdir->hibt != p->iodirb.byte)
        return -3;
    return 0;
}

int32_t mcp23s17_write(mcp23s17_t *p, uint8_t regaddr, uint8_t regvalue)
{
    uint8_t tbuf[3] = {WR_CMD(p->devaddr), regaddr, regvalue};
    uint8_t rbuf[3];
    if (0 != mcp_spirw_buf(tbuf, rbuf, 3))
        return -1;
    return 0;
}

int32_t mcp23s17_read(mcp23s17_t *p, uint8_t regaddr, uint8_t *pregvalue)
{
    uint8_t tbuf[3] = {RD_CMD(p->devaddr), regaddr, 0};
    uint8_t rbuf[3];
    if (0 != mcp_spirw_buf(tbuf, rbuf, 3))
        return -1;
    *pregvalue = rbuf[2];
    return 0;
}

int32_t mcp23s17_write16(mcp23s17_t *p, uint8_t regaddr, uint16_t regvalue)
{
    uint8_t tbuf[4] = {WR_CMD(p->devaddr), regaddr, regvalue, (regvalue >> 8)};
    uint8_t rbuf[4];
    if (0 != mcp_spirw_buf(tbuf, rbuf, 4))
        return -1;
    return 0;
}

int32_t mcp23s17_read16(mcp23s17_t *p, uint8_t regaddr, uint16_t *pregvalue)
{
    uint8_t tbuf[4] = {RD_CMD(p->devaddr), regaddr, 0};
    uint8_t rbuf[4];
    if (0 != mcp_spirw_buf(tbuf, rbuf, 4))
        return -1;
    *pregvalue = (rbuf[3] * 256) | rbuf[2];
    return 0;
}
